#include "protocol.h"
#include "bluetooth.h"


#define TX_FRAME_HEAD	0x7a
#define TX_FRAME_END	0x7b
#define TX_CMD_CHN		0x01

#define RX_FRAME_HEAD		0x7b
#define RX_FRAME_END		0x7a
#define RX_CMD_PID			0x01
#define RX_CMD_MEDIUM		0x02

#define MAX_BUF		64
static u8 buf[MAX_BUF];
static u8 buf_idx = 0;
static u8 buf_len = 0;

static void send(unsigned char dat) {
	BT_send_byte(dat);
}

typedef union {
    float f;
    unsigned char b[4];
} FloatBytes;

static void floatToBytes(float value, unsigned char *bytes) {
    FloatBytes fb;
    fb.f = value;
    bytes[0] = fb.b[0];
    bytes[1] = fb.b[1];
    bytes[2] = fb.b[2];
    bytes[3] = fb.b[3];
}

static float bytesToFloat(unsigned char *bytes) {
    FloatBytes fb;
    fb.b[0] = bytes[0];
    fb.b[1] = bytes[1];
    fb.b[2] = bytes[2];
    fb.b[3] = bytes[3];
    return fb.f;
}

void Protocol_init() {
		BT_init();
}

void Protocol_push(float *chns, int chn_cnt) {
    //帧头  |   命令  |   数据长度   |   数据  |   校验码 | 帧尾
    //1字节 |   1字节 |   1字节      |  n字节  |  1字节   | 1字节
    //命令: 请求类型的标识
    //数据长度: 表示后面 数据 的字节个数
    //校验码:  命令 + 数据长度 + 数据, 取高位
    unsigned char i;
    float chn;
    unsigned char tmp[4];
    u16 sum = 0;

    send(TX_FRAME_HEAD);
    send(TX_CMD_CHN);
    send(chn_cnt * 4);
    sum += TX_CMD_CHN;
    sum += chn_cnt * 4;
    for(i = 0; i < chn_cnt; i++) {
        floatToBytes(chns[i], tmp);

        send(tmp[0]);
        sum += tmp[0];
        send(tmp[1]);
        sum += tmp[1];
        send(tmp[2]);
        sum += tmp[2];
        send(tmp[3]);
        sum += tmp[3];
    }
    send((sum >> 8) & 0xFF);
    send(TX_FRAME_END);
}

static void parse_config_pid() {
		float kp, ki, kd;
		kp = bytesToFloat(&buf[(buf_idx + 4) % MAX_BUF]);
		ki = bytesToFloat(&buf[(buf_idx + 8) % MAX_BUF]);
		kd = bytesToFloat(&buf[(buf_idx + 12) % MAX_BUF]);
		smc_operation();
}

void Protocol_parse() {
    u8 dat_len, i;
    u16 sum = 0;
    //////////////// check cmd
    if(buf_len < 3) return;

    // 消息组成
    // 帧头  |   命令  |   数据长度    |   数据  |   校验码 | 帧尾
    // 1字节 |   1字节 |   1字节       |  n字节  |  1字节   | 1字节
    // 命令: 请求类型的标识
    // 数据长度: 表示后面 数据 的字节个数
    // 校验码:  命令 + 数据长度 + 数据, 取高位

    // check frame head
    if(buf[buf_idx] != RX_FRAME_HEAD) {
        // drop one
        buf_idx++;
        buf_idx %= MAX_BUF;
				if(buf_len != 0) buf_len--;

        Protocol_parse();
        return;
    }
		
    // check dat len
    dat_len = buf[(buf_idx + 2) % MAX_BUF];
    // check cmd total len
    if(buf_len < dat_len + 5) return;

    // check sum
    sum += buf[(buf_idx + 1) % MAX_BUF];
    sum += buf[(buf_idx + 2) % MAX_BUF];
    for(i = 0; i < dat_len; i++) {
			sum += buf[(buf_idx + 3 + i) % MAX_BUF];
    }
	
		if(((sum >> 8) & 0xFF) != buf[(buf_idx + dat_len + 3) % MAX_BUF]) {
				// drop
				buf_idx++;
        buf_idx %= MAX_BUF;
        if(buf_len != 0) buf_len--;

        Protocol_parse();
        return;
		}
		// check frame end
		if(buf[(buf_idx + dat_len + 4) % MAX_BUF] != RX_FRAME_END) {
				// drop
				buf_idx++;
        buf_idx %= MAX_BUF;
        if(buf_len != 0) buf_len--;

        Protocol_parse();
        return;
		}
		// parse success
		u8 cmd = buf[(buf_idx + 1) % MAX_BUF];
		
		u8 data_len = 5;
		
		switch(cmd){
            // TODO: need add code from Superior-Machine-Control-Operation command
			case RX_CMD_PID:
				// config pid
				// parse_config_pid();
				// data_len += (1 + 3 * 4);
				break;
			case RX_CMD_MEDIUM:
				// parse_medium_value();
				// data_len += 4;
				break;			
			default:
				break;
		}
		if (data_len > 5){		
			// drop
			buf_idx += data_len;
			buf_idx %= MAX_BUF;
			if(buf_len >= data_len) {
					buf_len -= data_len;
			} else {
					buf_len = 0;
			}
		}
		
		Protocol_parse();
}

void Protocol_pull(unsigned char dat) {
    u8 idx = buf_idx + buf_len;
    idx %= MAX_BUF;
    // config dat
    buf[idx] = dat;
    buf_len ++;
		
		if(buf_len > MAX_BUF) {
			buf_idx = 0;
			buf_len = 0;
		}
}